Three main categories:
Vision-based
Infrared/Lidar-based
Radio-based
Vision based, like Visual Inertial Navigation suffers low visibility conditions and presence of dust or fog.
Radio-Frequency solutions advantages:
Robust to lights changes, dust and fog
Low computational cost
In the analysis, we will consider the following model for the measurement:
\[ \mathbf{b} = h(\mathbf{q}) + \mathbf{w} \]
where \(\mathbf{q} \in \mathbb{R}^n\) is in general the position to be found, with \(n = 2\) for 2D problems, \(\mathbf{b} \in \mathbb{R}^m\) is the vector of the \(m\) measurements, and \(\mathbf{w} \in \mathbb{R}^m\) is the vector of the sensor uncertainties.
Recalling the model for the measurement:
\[ b_i = h_i(\mathbf{q}) + w_i. \]
Saying \(w\) to be negligible, the model can be given by:
\[ h_i(\mathbf{q}) = \rho_i = \sqrt{(\mathbf{q} - \mathbf{s}_i)^T (\mathbf{q} - \mathbf{s}_i)}, \]
where \(q\) is the position to be found, \(s_i\) is the position of the \(i\)-th anchor, and \(\rho_i\) is the distance between the \(i\)-th anchor and the tag.
\[ \mathbf{q} = \begin{bmatrix} x \\ y \end{bmatrix}, \quad \mathbf{s}_i = \begin{bmatrix} x_i \\ y_i \end{bmatrix}. \]
It is now evident that the possible positions of the tag can be geometrically interpreted as a circle. Indeed, we immediately have:
\[ \rho_i^2 = (x - x_i)^2 + (y - y_i)^2. \]
Moreover, with simple algebraic observations, it turns out that at least three base stations are needed.
Since we have \(m\) base stations, we have the vectorial representation:
\[ \mathbf{b}_m = h(q) + \mathbf{w}_m, \]
where, as in the previous cases,
\[ \mathbf{b}_m = \begin{bmatrix} b_1 \\ b_2 \\ \vdots \\ b_m \end{bmatrix}, \quad h(\mathbf{q}) = \rho = \begin{bmatrix} h_1(\mathbf{q}) \\ h_2(\mathbf{q}) \\ \vdots \\ h_m(\mathbf{q}) \end{bmatrix}. \]
The objective of an estimator is to retrieve the estimate \(\hat{q}\) and possibly a measure of its uncertainty. For the ToA approach, static estimators can be:
\[ J(\hat{q}) = \sum_{i=1}^{m} \frac{(b_i - \sqrt{(\hat{x} - x_i)^2 + (\hat{y} - y_i)^2})^2}{\sigma_i^2}; \]
Maximum Likelihood, which turns out to be a Nonlinear Weighted Least Squares in the case of Gaussian noises;
Least Squares on a linearized version of the measurements.
For the latter case, let us consider the squares of the measurements:
\[ b_i^2 = (\hat{x} - x_i)^2 + (\hat{y} - y_i)^2. \]
expanding the squares, we have:
\[ b_i^2 = \hat{x}^2 - 2x_i\hat{x} + x_i^2 + \hat{y}^2 - 2y_i\hat{y} + y_i^2. \]
By defining \(r = x^2 + y^2\), we can rewrite the previous expression as:
\[ b_i^2 - x_i^2 - y_i^2 - z_i^2 = r - 2xx_i - 2yy_i - 2zz_i + \eta_i. \]
By defining:
\[ \mathbf{b}_m^* = \begin{bmatrix} b_1^2 - x_1^2 - y_1^2 \\ b_2^2 - x_2^2 - y_2^2 \\ \vdots \\ b_m^2 - x_m^2 - y_m^2 \end{bmatrix}, \quad \boldsymbol{\theta} = \begin{bmatrix} x \\ y \\ r \end{bmatrix}, \quad \mathbf{H} = \begin{bmatrix} -2x_1 & -2y_1 & 1 \\ -2x_2 & -2y_2 & 1 \\ \vdots & \vdots & \vdots \\ -2x_m & -2y_m & 1 \end{bmatrix}, \]
we can rewrite the previous expression as:
\[ \mathbf{b}_m^* = \mathbf{H}\boldsymbol{\theta}. \]
Notice that in the previous example both the estimates \(\mathbf{\hat{q}}\) and \(\hat{r}\) are derived. Since \(\hat{r} = \mathbf{\hat{q}}^T \mathbf{\hat{q}}\), this constraint should be enforced and hence constrained Least Squares solutions should be applied.
Alternatively, we can get rid of \(r\) by computing \(b_i^2 - b_j^2\), i.e.,
\[ b_i^2 - b_j^2 - x_i^2 + x_j^2 - y_i^2 + y_j^2 = -2(x_i - x_j)x - 2(y_i - y_j)y + \eta_i - \eta_j. \]
As in the previous case, we can derive the matrix formulation for this problem as well, which, again, requires just the Least Squares to be computed.
Using linear algebra is possible to find the solution of the Least Square inverting the matrix \(H\)
\[ \mathbf{b} = \mathbf{H} \mathbf{p} \quad \rightarrow \quad \mathbf{p} = \mathbf{H}^{-1} \mathbf{b}. \]
This is true if the matrix \(\mathbf{H}\) is square and full rank. If the matrix is rectangular, the pseudo-inverse should be computed instead of the inverse with the Moore-Penrose formula \(\mathbf{H}^+ = (\mathbf{H}^T \mathbf{H})^{-1} \mathbf{H}^T\).
Using the trick of the previous slide we can rewrite the matrix \(b\) and \(H\) for the case with 3 anchors as
\[ \bar{\mathbf{b}} = \begin{bmatrix} b_1^2 - b_2^2 - x_1^2 + x_2^2 - y_1^2 + y_2^2 \\ b_2^2 - b_3^2 - x_2^2 + x_3^2 - y_2^2 + y_3^2 \end{bmatrix}, \quad \bar{\mathbf{H}} = \begin{bmatrix} -2x_1 + 2x_2 & -2y_1 + 2y_2 \\ -2x_2 + 2x_3 & -2y_2 + 2y_3 \end{bmatrix}. \]
So the solution can be computed as:
\[ \bar{\mathbf{b}} = \bar{\mathbf{H}} \mathbf{p} \quad \rightarrow \quad \mathbf{p} = \bar{\mathbf{H}}^{-1} \bar{\mathbf{b}}. \]
If the matrix is not full rank, the solution is not unique. It can see geometrically that when the anchors are aligned, the matrix is not full rank. In this case, the solution is not unique and the problem is ill-posed.
The distance between the tag and the anchor can be estimated using different techniques. The most common are:
We assume that the target node emits a signal at time \(t\) that is received by the \(i\)-th base station at time \(t_i\).
The distance \(\rho_i\) is then given by:
\[ \rho_i = (t_i - t)c, \]
where \(c\) is typically the velocity of light \(3 \cdot 10^8\) m/s (e.g., for electromagnetic signals) or the velocity of sound 340 m/s (e.g., for ultrasound systems). From the previous expression, it turns out the necessity of node synchronization, while the quantity \((t_i - t)\) is the Time of Flight (ToF).
ToA has some drawbacks:
We introduce now the Time Difference of Arrival (TDoA). As for the ToA, the ToA comprises the target node and a set of base stations, whose positions are assumed to be known.
TDoA, as ToA, is an active technique and the position is either computed by the element being localised or by the base stations. Similarly, in coplanar cases, just three base stations are needed, otherwise four such nodes are needed.
Basically ToA and TDoA share the same configuration. So, what are the differences?
Let us again model the TDoA for the planar case. We consider the position to be determined \(\mathbf{q} = [x, y]^T\) and \(\mathbf{s}_i = [x_i, y_i]^T\) the positions of \(i = 1, \ldots, m\) base stations. The target node emits a signal at time \(t\) that is received by the \(i\)-th base station at time \(t_i\).
The distance \(\rho_i\) is again given by:
\[ \rho_i = (t_i - t)c, \]
but now we assume that \(t\) is unknown to the base stations. However, considering the difference of times when two base stations receive the message, then:
\[ t_i - t_j = (t_i - t) - (t_j - t) = \frac{\rho_i}{c} - \frac{\rho_j}{c}, \]
where \(t_i - t_j = \delta_{ji}\) is the measured TDoA.
Recalling the measurement model:
\[ b_{ji} = h_{ji}(q) + w_{ji}, \]
it is evident that \(b_{ji} = \rho_{ji}\) (distance between the base station \(i\) and the base station \(j\)), \(w_{ji} \sim \mathcal{N}(0, \sigma^2_{ji})\), and the measurement model can be given by:
\[ h_{ji}(q) = \rho_{ji} = \sqrt{(q - s_i)^T (q - s_i)} - \sqrt{(q - s_j)^T (q - s_j)}, \]
since \(\mathbf{q} = [x, y]^T\) and \(\mathbf{s}_i = [x_i, y_i]^T\) and \(\mathbf{s}_j = [x_j, y_j]^T\) are the positions of the \(i\)-th and \(j\)-th base stations.
It is now evident why the TDoA can be geometrically interpreted as the locus of all points \((x, y)\) such that the difference of the distances from \(\mathbf{s}_i\) and \(\mathbf{s}_j\) is constant and equal to \(\rho_{ji}\). Hence, it turns out that at least three base stations are needed for the planar case.
TDoA has the following advantages with respect to ToA:
TDoA still has the following drawbacks:
Ultra-wide band (UWB) refers to the wireless technology that can access the frequency spectrum larger than 500 MHz, usually in the range 3 to 10 GHz.
UWB has unique advantages, including wall penetration capability, low transmission power, simple transceiver structure, and high temporal as well as spatial resolutions. These features potentially allow centimeter-level positioning accuracy with high precision.
We are not going into the details of the solution, what is of interest to us are the positioning techniques that can be adopted.
Usually, UWB is adopted for ToA or TDoA approaches. UWB uses two different communication techniques:
• Impulse Radio: transmission of pulses that occupy the entire bandwidth.
• Multi-band Orthogonal Frequency Division Multiplexing: the spectrum is used to transmit several symbols on several sub-bands.
Given the different technique, the algorithm to extract the timing information changes, but for our purposes nothing changes.
When trying to estimate the position of a moving target, the problems become more complex. An additional term should be considered in the model, i.e., the orientation of the target. The new coordinates are \(\mathbf{q} = [x, y, \theta]^T\), where:
The control variable of the system are:
The dynamics of the system can be given by:
\[ \begin{aligned} \dot{x} &= v \cos(\theta), \\ \dot{y} &= v \sin(\theta), \\ \dot{\theta} &= \omega. \end{aligned} \]
To simulate numerically, we discretize with time step \(\Delta t\):
\[ \begin{cases} x_{k+1}=x_k+v_k\cos\theta_k\,\Delta t \\ y_{k+1}=y_k+v_k\sin\theta_k\,\Delta t \\ \theta_{k+1}=\theta_k+\omega_k\,\Delta t \end{cases} \]
Localization of a master using only ranging information from UWB anchors.
Given the points \[ \mathbf{N} = \begin{bmatrix} \mathbf{N}_0 & \cdots & \mathbf{N}_n \end{bmatrix} = \begin{bmatrix} x_0 & \cdots & x_n \\ y_0 & \cdots & y_n \end{bmatrix}, \]
where \(\mathbf{N}_i\) is the position of the \(i\)-th anchor. Let us assume that the \(i\)-th node has access to the distances \[ \rho_{i,j} = \|\mathbf{N}_i - \mathbf{N}_j\| = \sqrt{(x_i - x_j)^2 + (y_i - y_j)^2}, \] so that the symmetric squared Euclidean distance matrix \[ \mathbf{D} = \begin{bmatrix} 0 & \rho^2_{0,1} & \cdots & \rho^2_{0,n} \\ \rho^2_{1,0} & 0 & \cdots & \rho^2_{1,n} \\ \vdots & \vdots & \ddots & \vdots \\ \rho^2_{n,0} & \rho^2_{n,1} & \cdots & 0 \end{bmatrix}, \] can be built. This is the matrix that we have access to.
Using the double centering matrix \[ \mathbf{H} = \mathbf{I}_{n+1} - \frac{1}{n+1} \mathbf{e}\mathbf{e}^T, \] where \(\mathbf{e}\mathbf{e}^T = \mathbf{1}_{n+1} \mathbf{1}_{n+1}^T\), \(\mathbf{1}_{n+1}\) is a column vector filled with \(n+1\) ones and \(\mathbf{I}_{n+1}\) is the identity matrix of dimension \(n+1 \times n+1\), to transform \(\mathbf{D}\), we obtain the Gram matrix
\[ \mathbf{G} = -\frac{1}{2} \mathbf{H}\mathbf{D}\mathbf{H}. \]
This transformation turns pairwise Euclidean distances into pairwise inner products of vectors.
Let us define \(\mathbf{P} = \begin{bmatrix} \mathbf{p}_0 & \mathbf{p}_1 & \cdots & \mathbf{p}_n \end{bmatrix}^T\) as the matrix of node coordinates that generates the symmetric Euclidean matrix \(\mathbf{D}\), and that is a replica of \(\mathbf{N}\) but affected by geometric ambiguities.
The Gram matrix \(\mathbf{G}\) has a special structure: if \(\mathbf{G}\) is positive semi-definite, it can be decomposed into the product of coordinates (or vectors) that represent the points. Specifically, if \(\mathbf{G} = \mathbf{P} \mathbf{P}^T\), where \(\mathbf{P}\) contains the coordinates of the points we’re trying to find, then \(\mathbf{G}\) preserves the pairwise distances in the following sense:
\[ \mathbf{G}_{ij} = \langle \mathbf{p}_i, \mathbf{p}_j \rangle = \mathbf{p}_i^T \mathbf{p}_j, \]
This means that each entry \(\mathbf{G}_{ij}\) in the Gram matrix corresponds to the inner product of the coordinates of points \(\mathbf{p}_i\) and \(\mathbf{p}_j\). Since inner products determine the relative geometry of points, reconstructing \(\mathbf{P}\) will preserve these distances up to an affine transformation.
To derive \(\mathbf{P}\), the following optimization problem needs to be solved: \[ \arg \min_{\mathbf{P}} \| \mathbf{G} - \mathbf{P} \mathbf{P}^T \|^2. \] The solution to this optimization problem is given by the eigen-decomposition, i.e. \[ \mathbf{P} = \begin{bmatrix} \mathbf{p}_0 & \cdots & \mathbf{p}_n \end{bmatrix} = \begin{bmatrix} \tilde{x}_0 & \cdots & \tilde{x}_n \\ \tilde{y}_0 & \cdots & \tilde{y}_n \end{bmatrix} = \mathbf{U} \sqrt{\mathbf{V}}, \] where \(\mathbf{V}\) is the diagonal matrix of the eigenvalues, and \(\mathbf{U}\) is the eigenvector matrix of \(\mathbf{G}\).
Specifically:
\(\mathbf{G} = \mathbf{U} \mathbf{V} \mathbf{U}^T,\) where \(\mathbf{U}\) is the matrix of eigenvectors and \(\mathbf{V}\) is the diagonal matrix of eigenvalues.
By taking \(\mathbf{P} = \mathbf{U} \sqrt{\mathbf{V}},\) we construct coordinates that satisfy \(\mathbf{G} \approx \mathbf{P} \mathbf{P}^T,\) meaning that \(\mathbf{P}\) will reproduce the distances in \(\mathbf{D}\) up to rotations, reflections, or translations.
The resulting points \(\mathbf{P}\) are not exactly the same as the original points \(\mathbf{N}\); they are affine transformations of \(\mathbf{N}\), which means that they could be rotated, reflected, or translated versions of \(\mathbf{N}\). This is because the Gram matrix \(\mathbf{G}\) preserves distances, but it does not preserve absolute positioning or orientation in space. However, any two point sets \(\mathbf{N}\) and \(\mathbf{P}\) that produce the same \(\mathbf{G}\) will have identical pairwise distances, which is the key requirement for applications relying on \(\mathbf{D}\).
More precisely, if there exists an angle \(\theta \neq 2k\pi\) with \(k \in \mathbb{N}\) such that \[ \mathbf{N} = \begin{bmatrix} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{bmatrix} \mathbf{P} = \mathbf{R}(\theta) \mathbf{P}, \] then a rotation ambiguity occurs. The flipping problem takes place if \[ \mathbf{N} = \pm \begin{bmatrix} -1 & 0 \\ 0 & 1 \end{bmatrix} \mathbf{P} = \pm \mathbf{S} \mathbf{P}. \]
Until now we understood that given the distances is possible to reconstruct the position of the anchors up to a rotation, reflection, or translation. So we want to know if taking more measurements will help to find the relative position of the master with respect to the anchors.
Position of the master and two anchors at time \(t_1\).
Position of the master and two anchors at time \(t_2\).
Position of the master and two anchors at time \(t_3\).
Consider three time instants \(k\), \(k + 1\), and \(k + 2\) where the moving node \(\mathbf{N}_0\):
Here, \(\mathbf{t}_k = \begin{bmatrix} \Delta x_k \\ \Delta y_k \end{bmatrix}\) and \(\mathbf{t}_{k+1} = \begin{bmatrix} \Delta x_{k+1} \\ \Delta y_{k+1} \end{bmatrix}\) are translation vectors.
With measurements \(\rho_{i,j} + \eta_{i,j}\) for three consecutive time instants:
We build distance matrices \(\mathbf{D}^k\), \(\mathbf{D}^{k+1}\), and \(\mathbf{D}^{k+2}\).
Use these distance matrices to compute estimated positions \(\hat{\mathbf{P}}^k\), \(\hat{\mathbf{P}}^{k+1}\), and \(\hat{\mathbf{P}}^{k+2}\) by solving the optimization problem: \[ \arg \min_{\mathbf{P}} \| \mathbf{G} - \mathbf{P} \mathbf{P}^T \|^2 \]
After alignment, the estimated translation \(\mathbf{T}\) provides an approximation of the actual displacement \(\mathbf{t}_k\) in the opposite direction.
This alignment can be extended from \(\hat{\mathbf{P}}^k\) and \(\hat{\mathbf{P}}^{k+1}\) to the next time step \(\hat{\mathbf{P}}^{k+2}\), yielding the path of the moving node \(\mathbf{N}_0\) in its relative frame centered on \(\hat{\mathbf{p}}_{0,k}\).
Theorem: Given a set of \(m > 0\) node \(0\) motions, \[ \mathbf{N}_0^{k+q} = \mathbf{N}_0^{k+q-1} + \mathbf{t}_{k+q-1}, \quad q = 1, \ldots, m \] it is impossible to determine \(\alpha \mathbf{S}\) (flipping operation) if we have no additional knowledge about \(\mathbf{t}_{k+q-1}\).
Corollary: - By knowing the sign of angle \(\beta = \arctan \frac{\Delta y_{k+1} - \Delta y_k}{\Delta x_{k+1} - \Delta x_k}\), we can resolve flipping. - The angle \(\beta\) gives the relative rotation direction of \(t_k\) to \(t_{k+1}\), indicating whether the movement is clockwise or counterclockwise.